
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_l1_control.h
  * @author     baiyang
  * @date       2023-7-13
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "mb_navigation.h"

#include <common/gp_defines.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct mb_l1_control* mb_l1_control_t;
typedef const struct mb_l1_control* mb_l1_control_const_t;

struct mb_l1_control {
    struct navigation_controller navctrl;

    // lateral acceration in m/s required to fly to the
    // L1 reference point (+ve to right)
    float _latAccDem;

    // L1 tracking distance in meters which is dynamically updated
    float _L1_dist;

    // Status which is true when the vehicle has started circling the WP
    bool _WPcircle;

    // bearing angle (radians) to L1 point
    float _nav_bearing;

    // bearing error angle (radians) +ve to left of track
    float _bearing_error;

    // crosstrack error in meters
    float _crosstrack_error;

    // target bearing in centi-degrees from last update
    int32_t _target_bearing_cd;

    // L1 tracking loop period (sec)
    Param_float _L1_period;
    // L1 tracking loop damping ratio
    Param_float _L1_damping;

    // previous value of cross-track velocity
    float _last_Nu;

    // integral feedback to correct crosstrack error. Used to ensure xtrack converges to zero.
    // For tuning purposes it's helpful to clear the integrator when it changes so a _prev is used
    float _L1_xtrack_i;
    Param_float _L1_xtrack_i_gain;
    float _L1_xtrack_i_gain_prev;
    uint32_t _last_update_waypoint_us;
    bool _data_is_stale;

    Param_float _loiter_bank_limit;

    bool _reverse;
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void l1_control_ctor(mb_l1_control_t l1_ctrl);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



